#include "zf_common_headfile.h"

uint16 ad_value[6];    //�ĸ����ADֵ�洢
uint16 Filt_ADvalue[6];
float X_Deviation,Y_Deviation,XY_Deviation_Old=0,XY_Deviation=0,XY_Deviation_Error;
float M_NormalValue=0,X_NormalDeviation=0,Y_NormalDeviation=0;
float Ring_In_adave=330;

float in_ad_value=0, Ring_Recode=0;

float  ElEC_KiXY= 0.5;
float  ElEC_KiX= 2.0;
float  ElEC_KiY= 0.55;

//**********Ԫ�ر�־*********
int low_pass=1;//�е���˲���־
int induc_diff=0;//���ҵ�в�
char induc_add=0;//���ҵ�к�
//************ʹ��**************

float buff_mid[6];
float Ring_In_Ele[3],Ring_In_count=0;
float Weight_filtering_x1[4],Weight_filtering_x2[4],Weight_filtering_y1[4],Weight_filtering_y2[4];
float x1,x2,y1,y2,XY_Deviation_D[6],t1,t2;
float Ring_x1,Ring_x2,Ring_y1,Ring_y2,Ring_t1,Ring_t2,Ring_m1;
uint8 Ring_count_x=0,Ring_count_y=0,Ring_count_t=0;
float voltage=0;

int Ramp_time = 0;

/******************************************
*adc��ѹ��ȡ
******************************************/
void ad_voltage(void)
{
    float ad_voltage=0;
    adc_init(ADC0_CH6_A6, ADC_12BIT);
    ad_voltage = adc_mean_filter_convert(ADC0_CH6_A6,10);//�ɼ�10�� Ȼ�󷵻�ƽ��ֵ
    //voltage = ad_voltage*6.00f/4095*3.3f;
    voltage = ad_voltage*5.32f/4095*3.3f+0.78;
    //12.6V电池
    voltage = ad_voltage/4095/0.35f*12.6;
}

void ad_init(void)
{
    adc_init(ADC0_CH0_A0, ADC_12BIT);
    adc_init(ADC0_CH1_A1, ADC_12BIT);   //�ҵ��
    adc_init(ADC0_CH2_A2, ADC_12BIT);   //�е��
    adc_init(ADC0_CH3_A3, ADC_12BIT);   //����
    adc_init(ADC0_CH4_A4, ADC_12BIT);
}

/******************************************
*adc�˲�
******************************************/
float xx1=1,xx2=1,yy1=1,yy2=1,tt1=1,tt2=1;
void ad_filt(void)
{

    uint8 i;
//    uint16 ad_value_MidTemp[6][3]= {0};
//    uint16 ad_value_temp[6][3]= {0};
//  if(Rise_Angle>=0) Rise_Angle = 0;

//    ad_value[0] = adc_mean_filter(ADC_2, ADC2_CH5_A37, ADC_12BIT, 10);//�Һ���
//    ad_value[1] = adc_mean_filter(ADC_2, ADC2_CH4_A36, ADC_12BIT, 10);//����ֱ
//    ad_value[2] = adc_mean_filter(ADC_2, ADC2_CH3_A35, ADC_12BIT, 10);//�м���
//    ad_value[3] = 1;
//    ad_value[4] = adc_mean_filter(ADC_1, ADC1_CH9_A25, ADC_12BIT, 10);//����ֱ
//    ad_value[5] = adc_mean_filter(ADC_1, ADC1_CH8_A24, ADC_12BIT, 10);//�����

    ad_value[0] = adc_mean_filter_convert(ADC0_CH0_A0, 10);//�����
    ad_value[1] = adc_mean_filter_convert(ADC0_CH1_A1, 10);//����ֱ
    ad_value[2] = adc_mean_filter_convert(ADC0_CH2_A2, 10);//�м���
    ad_value[3] = 1;
    ad_value[4] = adc_mean_filter_convert(ADC0_CH3_A3, 10);//����ֱ
    ad_value[5] = adc_mean_filter_convert(ADC0_CH4_A4, 10);//�Һ���

    for(i = 0; i < 6; i++)
    {
        if( i != 1 || i != 4 )
            Filt_ADvalue[i] = ad_value[i];
    }

    //*****************�Һ����˲�******************************/
    Weight_filtering_x1[0] = Weight_filtering_x1[1];
    Weight_filtering_x1[1] = Weight_filtering_x1[2];
    Weight_filtering_x1[2] = Weight_filtering_x1[3];
    Weight_filtering_x1[3] = ad_value[0];
    //******************������˲�****************************/
    Weight_filtering_x2[0] = Weight_filtering_x2[1];
    Weight_filtering_x2[1] = Weight_filtering_x2[2];
    Weight_filtering_x2[2] = Weight_filtering_x2[3];
    Weight_filtering_x2[3] = ad_value[5];
    //******************����ֱ����˲�****************************/
    Weight_filtering_y1[0] = Weight_filtering_y1[1];
    Weight_filtering_y1[1] = Weight_filtering_y1[2];
    Weight_filtering_y1[2] = Weight_filtering_y1[3];
    Weight_filtering_y1[3] = ad_value[1];
    //******************����ֱ����˲�****************************/
    Weight_filtering_y2[0] = Weight_filtering_y2[1];
    Weight_filtering_y2[1] = Weight_filtering_y2[2];
    Weight_filtering_y2[2] = Weight_filtering_y2[3];
    Weight_filtering_y2[3] = ad_value[4];

    //****************��Ȩ�ó�ƽ��ֵ*********************/
    x1 = Weight_filtering_x1[3] * 0.4 + Weight_filtering_x1[2] * 0.3 + Weight_filtering_x1[1] * 0.2 + Weight_filtering_x1[0] * 0.1;
    x2 = Weight_filtering_x2[3] * 0.4 + Weight_filtering_x2[2] * 0.3 + Weight_filtering_x2[1] * 0.2 + Weight_filtering_x2[0] * 0.1;
    y1 = Weight_filtering_y1[3] * 0.4 + Weight_filtering_y1[2] * 0.3 + Weight_filtering_y1[1] * 0.2 + Weight_filtering_y1[0] * 0.1;
    y2 = Weight_filtering_y2[3] * 0.4 + Weight_filtering_y2[2] * 0.3 + Weight_filtering_y2[1] * 0.2 + Weight_filtering_y2[0] * 0.1;
    t1 = sqrt(x1 * x1 + y1 * y1);
    t2 = sqrt(x2 * x2 + y2 * y2);
    xx1 = (float)((int)x1 / 10 * 10);
    xx2 = (float)((int)x2 / 10 * 10);
    yy1 = (float)((int)y1 / 10 * 10);
    yy2 = (float)((int)y2 / 10 * 10);
    tt1 = (float)((int)t1 / 100 * 100);
    tt2 = (float)((int)t2 / 100 * 100);

    if(Ramp_Flag == 0&&yy1==yy2&&yy1>150&&yy2>150&&yy1<350&&yy2<350)
    {
        if(xx1 == xx2)
        {
            if(Ring_count_x == 0)
            {
                Ring_x1 = x1;
                Ring_x2 = x2;
                Ring_count_x = 1;
                Ring_m1 = ad_value[2];
            }
            else
            {
                Ring_x1 = x1 * 0.02 + Ring_x1 * 0.98;
                Ring_x2 = x2 * 0.02 + Ring_x2 * 0.98;
                Ring_m1 = ad_value[2] * 0.02 + Ring_m1 * 0.98;
            }
        }

        if(yy1 == yy2)
        {
            if(Ring_count_y == 0)
            {
                Ring_y1 = y1;
                Ring_y2 = y2;
                Ring_count_y = 1;
            }
            else
            {
                Ring_y1 = y1 * 0.02 + Ring_y1 * 0.98;
                Ring_y2 = y2 * 0.02 + Ring_y2 * 0.98;
            }
        }

        if(tt1 == tt2)
        {
            if(Ring_count_t == 0)
            {
                Ring_t1 = t1;
                Ring_t2 = t2;
                Ring_count_t = 1;
            }
            else
            {
                Ring_t1 = t1 * 0.02 + Ring_t1 * 0.98;
                Ring_t2 = t2 * 0.02 + Ring_t2 * 0.98;
            }
        }
    }
    else
    {
        Ring_x1 = x1;
        Ring_x2 = x2;
        Ring_m1 = ad_value[2];
        Ring_t1 = t1;
        Ring_t2 = t2;
        Ring_y1 = y1;
        Ring_y2 = y2;
    }

}//��������

float mid_rate=0, t1_and_t2_rate = 0, y1_and_y2_rate = 0, y1_rate = 0, y2_rate = 0;

void ad_normal(void)
{
    uint8 i=0;
    float Ring_In_P = 1;
    float AD_LinearEquation[6];
    static float AD_MAX_OLD=0;
    ad_filt();

    for(i=0; i<6; i++)
    {
        AD_LinearEquation[i]=(float)(Filt_ADvalue[i]*1.0);
    }

    /*************** ��ͨ�˲�*************/
    if(Ring_Flag == 0 && Ramp_Flag == 0&&yy1==yy2&&yy1>110)
    {
//        if(low_pass == 1)
//        {
//            Ring_In_adave = AD_LinearEquation[2];
//            low_pass = 0;
//        }
//        else
            Ring_In_adave = Ring_In_adave * 0.98 + AD_LinearEquation[2] * 0.02;
    }

    //*****�м����˲�ֵ***����ʶ��Բ�����ص���//
    buff_mid[5] = buff_mid[4];
    buff_mid[4] = buff_mid[3];
    buff_mid[3] = buff_mid[2];
    buff_mid[2] = buff_mid[1];
    buff_mid[1] = buff_mid[0];
    buff_mid[0] = AD_LinearEquation[2];

    X_Deviation = 100 * ((x1 - x2) / (x1 + x2));                //����ƫ����
    X_Deviation = X_Deviation * 0.5;
    X_Deviation        = X_Deviation >  40 ?  40 : X_Deviation;
    X_Deviation        = X_Deviation < -40 ? -40 : X_Deviation;
    Y_Deviation = 100 * ((y1 - y2) / (y1 + y2));                //����ƫ����
    X_NormalDeviation = ElEC_KiX * X_Deviation;
    Y_NormalDeviation = ElEC_KiY * Y_Deviation;
    XY_Deviation = ElEC_KiXY * (X_NormalDeviation + 2 * (fabs(Y_NormalDeviation) > 10.0 ? (Y_NormalDeviation / fabs(Y_NormalDeviation) * (fabs(Y_NormalDeviation) - 10.0)) : 0)); //���ƫ����  ��ʹ����ʵ�ֲ���ת��
    if((ad_value[5]>ad_value[4]&&ad_value[2]<95&&ad_value[1]<100&&ad_value[0]<100&&AD_MAX_OLD<0)
            ||(ad_value[5]<100&&ad_value[4]<100&&ad_value[2]<95&&ad_value[1]<150&&ad_value[0]<150&&AD_MAX_OLD<0))
        {
            XY_Deviation=-80-(80+XY_Deviation);
        }
        else if((ad_value[0]>ad_value[1]&&ad_value[2]<95&&ad_value[5]<100&&ad_value[4]<100&&AD_MAX_OLD>0)
            ||(ad_value[5]<150&&ad_value[4]<150&&ad_value[2]<95&&ad_value[1]<100&&ad_value[0]<100&&AD_MAX_OLD>0))
        {
            XY_Deviation=80+(80-XY_Deviation);
        }
    if(fabs(XY_Deviation)>70)
        AD_MAX_OLD=XY_Deviation;
    mid_rate = AD_LinearEquation[2]/Ring_m1;
    t1_and_t2_rate = (t1+t2)/(Ring_t1+Ring_t2);
    y1_and_y2_rate = (y1+y2)/(Ring_y1+Ring_y2);
    y1_rate = y1/Ring_y1;
    y2_rate = y2/Ring_y2;

    if(ad_value[0]<120&&ad_value[1]<120&&ad_value[2]<120&&ad_value[3]<120&&ad_value[4]<120&&Garageout_step==2&&zhi_flag==0)
    {
        AimDis_Encoder++;
    }
    else
    {
        AimDis_Encoder=0;
    }

    if(AimDis_Encoder>30)
    {
        Bee_on();
        Balance_Test=0;
    }
    //----------------���ת��ƫ�����޷�------------------//
    XY_Deviation        = XY_Deviation >  120 ?  120 : XY_Deviation;
    XY_Deviation        = XY_Deviation < -120 ? -120 : XY_Deviation;
    //----------------���ת��ƫ�����޷�------------------//

    XY_Deviation_D[5]   = XY_Deviation_D[4];
    XY_Deviation_D[4]   = XY_Deviation_D[3];
    XY_Deviation_D[3]   = XY_Deviation_D[2];
    XY_Deviation_D[2]   = XY_Deviation_D[1];
    XY_Deviation_D[1]   = XY_Deviation_D[0];
    XY_Deviation_D[0]   = XY_Deviation;
    XY_Deviation_Error  = XY_Deviation_D[0] + XY_Deviation_D[1] + XY_Deviation_D[2] - (XY_Deviation_D[3] + XY_Deviation_D[4] + XY_Deviation_D[5]);
    XY_Deviation_Old    = XY_Deviation;

//    VOFA_ele_row[0] = XY_Deviation;
}
